import os
import sys
import serial
from serial.tools import list_ports
import threading
import queue
import time
import keyboard
import lib.MavlinkParse as MavlinkParse

TCP_IMPORT_MAVLINK = True  #是否使能Mavlink解析

"""
1、串口自动测试任务每5毫秒自动生成带计数的字符串，并将生成的数据保存到串口发送缓冲区
2、串口收发任务将发送缓冲区的数据通过串口设备发送出去
"""

"""
串口设备的名称和波特率随用户变化，按实际情况设置
"""
serial_port = "COM43"
serial_baud = 57600

"""
自动收发测试开启功能
"""
AUTO_TEST_ENABLE = False

serial_is_open = False
serial_task_start = False
serial_rec_queue = queue.Queue(0)
serial_send_queue = queue.Queue(0)

mav_parse = MavlinkParse.MavlinkParse()

### 串口收发任务
def serial_task(srl):
    global serial_is_open
    global serial_rec_queue
    global serial_task_start
    time.sleep(3)
    serial_task_start = True
    while True:
        try:
            if serial_is_open:
                if srl.readable():
                    data = srl.read(4096)
                    if len(data)>0:
                        serial_rec_queue.put((srl, data))

                        if TCP_IMPORT_MAVLINK:
                            try:
                                for t in data:
                                    m = mav_parse.parse([t,])
                                    if m is not None:
                                        print("Mavlink: %s->(id=%d, len=%d)" % (m.name, m.get_header().msgId, m.get_header().mlen))
                            except:
                                pass

                        print("Serial Rec: %d" % (len(data)))
                        #print("%s" % (str(data, "latin1"),), end='')
                if srl.writable():
                    #serial_send_queue.put(b'0123456789abcd\n')
                    while not serial_send_queue.empty():
                        data = serial_send_queue.get()[1]
                        #print("Serial Send: %d" % (len(data)))
                        srl.write(data)
        except:
            serial_is_open = False
            serial_rec_queue.queue.clear()
            serial_send_queue.queue.clear()
            print("Serial read task error!")
        time.sleep(0.005)

### 自动测试任务
def serial_auto_test_task():
    count = 0
    # sendstr = "0123456789"  #57600/8/1000 = 7.2B/ms
    while True:
        if serial_task_start:
            count += 1
            sendstr = "0123456789"*5 + ("--%d\n" % count)
            serial_send_queue.put((0, bytes(sendstr, "latin1")))
            print("Serial Send Data %d -> %d" % (len(sendstr), count))
        time.sleep(0.01)

### 监听按键，退出程序
def app_exit(ser):
    ser.close()
    print("Key: CTRL+X")
    os._exit(0)


if __name__ == '__main__':
    print("Serial List: ")
    sps = list_ports.comports()
    nameL =[]
    for a in sps:
        nameL.append(a.name.upper())
        print(a.name)

    if len(sys.argv)<=1:
        print("System argv error!!!")
        os._exit(0)

    serial_port = sys.argv[1].upper()
    if serial_port not in nameL:
        print("Serial Port %s Error!" % serial_port)
        os._exit(0)
    
    ser = serial.Serial(serial_port, serial_baud, timeout=0)

    print("Select Serial Port: %s" % (serial_port,))
    print("Select Serial Baud: %d" % (serial_baud,))
    print("If Exit: CTRL+X")

    keyboard.add_hotkey("ctrl+x", app_exit, args=(ser,))

    try:
        #ser.open()
        serial_is_open = True
        ser_td = threading.Thread(target=serial_task, args=(ser,))
        ser_td.start()

        if AUTO_TEST_ENABLE:
            ser_auto_td = threading.Thread(target=serial_auto_test_task, args=())
            ser_auto_td.start()
    except:
        ser.close()
        print("Serial Error!")

    while True:
        time.sleep(0.1)
